#pragma once
#include "RangeFinderArray.h"

struct PillboxColliderEssentials
{
	double m_p2Center[2]; // front and back points can be derived from this
	double m_lfLength;
	double m_lfWidth;
	double m_p2Dir[2];
};

class PillboxCollider
{
	double m_p2FrontPoint[2];
	double m_p2BackPoint[2];
	double m_lfWidth, m_lfLength;
	double m_p2Dir[2];
	double m_p2PerpDir[2];

	double m_arrLfBbox[4]; // 0 and 1 are min in x and y, 2 and 3 are max

	double m_arrLfRotRadByTheta[30]; // certainly we will need no more then this
	

	bool PointNearEnds(const double p2Point[2]) const;

	bool SegmentIsectInternal(const double p2SegStart[2], const double p2SegDir[2], const double lfSegLen) const;
	// p2SegDir must be normalized
	
	void CalcBbox();
		
	
	void SetDirParams();

	RangeFinderArray m_rangeFinder;

	double m_lfRangefinderMaxRange;
public:
	PillboxCollider(void);
	PillboxCollider(double lfLength, double lfWidth, double lfXPos, double lfYPos, double lfTheta);
	~PillboxCollider(void);

	void SetParams(double lfLength, double lfWidth, double lfXPos, double lfYPos, double lfTheta); // warning slow

	void SetPosition(double lfXPos, double lfYPos, double lfTheta);

	void SetRangefinders(double lfLength, double lfWidth, double lfAngleStep, int nNumPerSide)
	{
		m_rangeFinder.SetParams(lfLength, lfWidth, lfAngleStep, nNumPerSide);
	}

	void ResetRangefinders()
	{
		m_rangeFinder.ResetRanges(m_lfRangefinderMaxRange);

	}

	void SetRangefinderRange(double lfRange)
	{
		m_lfRangefinderMaxRange = lfRange;
	}

	void SetAngleStep(double lfAngleStep);

	void Rotate(double lfSinTheta, double lfCosTheta, double lfPerpDist); // rotate around back wheels
	// like a real car
	
	void GetDrawparams(double p2BackCenter[], double p2FrontCenter[], double p2DirNormed[]) const;

	inline void Translate(double lfDist) // translate forwards, along forward vector
	{
		double lfXDist = lfDist*m_p2Dir[0];
		double lfYDist = lfDist*m_p2Dir[1];
		m_p2FrontPoint[0] += lfXDist;
		m_p2FrontPoint[1] += lfYDist;
		m_p2BackPoint[0] += lfXDist;
		m_p2BackPoint[1] += lfYDist;
	}

	void GetGoalFromFront(const Pos2d p2Goal, Pos2d p2Out) const;

	void Move(int nAngleAmount, double lfDist);

	bool CollidePoint(const double p2PointToTest[2]) const;

	bool CollideSphere(const double p2PointToTest[2], double lfRadius) const;

	double DistanceToPoint(const double p2PointToTest[2]) const;

	bool CollideLine(const double p2PointLineStart[2], const double p2PointLineEnd[2]) const;

	bool CollideConvexPoly(const double arrP2PolyBoundary[][2], int nNumPts) const;
	
	void SetRaysForConvexPoly(const double arrP2PolyBoundary[][2], int nNumPts)
	{
		m_rangeFinder.CollideConvexPoly(arrP2PolyBoundary, nNumPts);
	}

	inline bool HitsBbox(const double * arrLfBbox) const
	{
		if(m_arrLfBbox[0] > arrLfBbox[2] ||
			m_arrLfBbox[2] < arrLfBbox[0] ||
			m_arrLfBbox[1] > arrLfBbox[3] ||
			m_arrLfBbox[3] < arrLfBbox[1])
		{
			return false;
		}
		return true;
	}

	inline const double * GetBbox() const { return m_arrLfBbox; }

	inline double GetLength() const { return m_lfLength; }
	inline double GetWidth() const { return m_lfWidth; }

	inline const RangeFinderArray & RangeFinders() const { return m_rangeFinder; }
};
